package oceanlife;
import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Point;
import java.awt.Shape;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Random;
import java.util.UUID;

import javax.swing.JPanel;

import oceanlife.pathfinding.Edge;
import oceanlife.pathfinding.Line2;
import oceanlife.pathfinding.Obstacles;
import oceanlife.pathfinding.PathArea;
import oceanlife.pathfinding.Pnt;
import oceanlife.pathfinding.PointGraph;
import oceanlife.pathfinding.RadialFace;
import oceanlife.pathfinding.SimpleGraphBuilder;
import oceanlife.pathfinding.SimpleObstacle;
import oceanlife.psychmodel.PsychComp;


public class Agent {
	private double halfPi = Math.PI/2.0;
	public RadialFace[] obsPoints = null;
	public ArrayList<Pnt> obsDistances = new ArrayList<Pnt>();
	public PathArea pathArea = new PathArea();
	private ArrayList<Agent> agents;
	private ArrayList<SimpleObstacle> simpleObs;

	public void setSimpleObs(ArrayList<SimpleObstacle> simpleObs) {
		this.simpleObs = simpleObs;
	}

	private Pnt pos;
	public static double radius = 5.84; //avg width of human = 58.4 cm = 11.68 pixels
	//private double xSize = 10.0;
	//private double ySize = 10.0;
	private double freeSpeed;
	private double currentSpeed = 0.0;
	private double direction;
	
	private double goffmanEllipseLength = 100.0; //5 cm = 1 pixel, 100 pixels = 5 meter
	private double goffmanEllipseWidth = 15.0;
	//distanceSafe, should be between 20-30 (scan ellipse = 2*ds && should be between 4-6 m) 
	private double ds = goffmanEllipseLength;
	
	private Pnt goal = new Pnt(0.0,0.0);
	
	private Pnt FSteer = new Pnt(0.0,0.0);

	public Pnt getFSteer() {
		return FSteer;
	}
	
	public double getDirection(){
		return direction;
	}

	private Pnt intermediateGoal = null;
	
	public Pnt getIntermediateGoal() {
		return intermediateGoal;
	}

	private double staticPressure = 0.0;
	private double socialPressure = 0.0;
	//private PsychComp psych = new PsychComp(this);
	
	public ArrayList<Pnt> obs = new ArrayList<Pnt>();
	private HashSet<Pnt> infAgents = new HashSet<Pnt>();
	private Color color;
	
	
	private Edge path = null;
	private int currentPointOnPath;
	
	private int cycles = 0; 
	private int lastRecalc = -1; 
	
	//private EdgePnt bestEdge;
	
	private AgentPositionGraphic agPosDraw;
	private AgentDirectionGraphic agDirDraw;
	private RandomGen rand = null;
	private UUID ID;

	public Agent(ArrayList<Agent> agents, Pnt[] spawnBox,RandomGen initrand){
		//this.simpleObs = simpleObs;
		this.agents = agents;
		this.pos = new Pnt(0.0,0.0);
		this.ID = UUID.randomUUID();

		this.pos.coord[0] = initrand.getRand(spawnBox[0].coord(0),spawnBox[1].coord(0));
		this.pos.coord[1] = initrand.getRand(spawnBox[0].coord(1),spawnBox[1].coord(1));
				
		//free speed is mean 1.56 m/s and sd 0.26
		this.freeSpeed = 15.6+(initrand.getGaussian()*2.6);
		this.direction = initrand.getRand(0.0,360.0);
		//this.direction = (float) 135;

		this.agPosDraw = new AgentPositionGraphic(pos.coord(0),pos.coord(1),radius*2,radius*2);
		this.agDirDraw = new AgentDirectionGraphic(pos.coord(0),pos.coord(1),freeSpeed,direction);
		
		//simPanel.add(agentDrawing);
		//agentDrawing.repaint();
	}
	
	//make duplicate agent. Need "agents" for context
	public Agent(ArrayList<Agent> agents,Agent ag, RandomGen rand) {
		this.rand = rand;
		this.agents = agents;
		this.pos = new Pnt(ag.getPosition());
		this.goal = new Pnt(ag.getGoal());
		this.color = ag.color;
		this.ID = ag.ID;
				
		this.freeSpeed = ag.freeSpeed;
		this.direction = ag.direction;

		this.agPosDraw = new AgentPositionGraphic(pos.coord(0),pos.coord(1),radius*2,radius*2);
		this.agDirDraw = new AgentDirectionGraphic(pos.coord(0),pos.coord(1),freeSpeed,direction);
		
		//copy references to objects that don't change during scenarios
		this.simpleObs = ag.simpleObs;
	}

	public Agent(UUID id, Pnt pos, Pnt goal, double freeSpeed, double direction, Color color) {
		this.pos = pos;
		this.ID = id;
		this.goal = goal;
		this.freeSpeed = freeSpeed;
		this.direction = direction;
		this.color = color;
		this.agPosDraw = new AgentPositionGraphic(pos.coord(0),pos.coord(1),radius*2,radius*2);
		this.agDirDraw = new AgentDirectionGraphic(pos.coord(0),pos.coord(1),freeSpeed,direction);
	}

	public Edge getPath(){
		return path;
	}
	
	public PathArea getPathArea() {
		return pathArea;
	}

	public double getDs(){
		return ds;
	}
	
	public void setGoal(Pnt goal) {
		this.goal = goal;
	}

	public double getX_coord() {
		return pos.coord[0];
	}
	
	public double getY_coord() {
		return pos.coord[1];
	}

	public double getFreeSpeed() {
		return freeSpeed;
	}
	
	public int getCycles(){
		return cycles;
	}
	
	public void move(SimpleGraphBuilder sGB) {
		updateIntermediateGoal(sGB);
		updateCoord();
		//running each cycle is to expensive. running every 0.1 sec
		if(cycles % (OLSet.getCps()/10) == 0){
			updateEdges(sGB);
		}
		agPosDraw.update(pos.coord[0], pos.coord[1], radius*2, radius*2);
		agDirDraw.update(pos.coord[0], pos.coord[1], freeSpeed, direction);
		cycles++;
	}

	private void updateEdges(SimpleGraphBuilder sGB) {
		//if(currentSpeed > 0){
			for(Pnt key : sGB.getGraph().nodeSet()){
				for(Pnt innerKey : sGB.getGraph().getTheNeighbors().get(key).keySet().toArray(new Pnt[0])){
					Edge ed = sGB.getGraph().getTheNeighbors().get(key).get(innerKey);
					if(ed.distanceTo(pos) < radius){
						ed.agentsOnEdge.add(this);
					} else {
						ed.agentsOnEdge.remove(this);
					}
					double efficiency = 0;
					for(Agent ag : ed.agentsOnEdge){
						efficiency += ag.getCurrentSpeed()/ag.getFreeSpeed();
					}
					if(ed.agentsOnEdge.size() > 0){
						efficiency /= (ed.agentsOnEdge.size()*1.0);
					} else {
						efficiency = 1.0;
					}
					//System.out.println(efficiency);
					ed.setEfficiency(efficiency);
				}
			}
		//}
		
	}

	private void updateIntermediateGoal(SimpleGraphBuilder sGB) {
		//polygon method
		//create path if non exists or if recalculated
		//1/(1+e^(-0.01(x-500)))
		//randtag
		if(rand == null){
			System.out.println("null");
		}
		int cyclesSinceLastRecalc = cycles - lastRecalc;
		double recalcChanceTime = 1.0/(1+Math.pow(Math.E,(-0.002*(cyclesSinceLastRecalc-5*OLSet.getCps()))));
		double recalcChanceTimeSpeed = recalcChanceTime / (1+99*(currentSpeed/freeSpeed));
		if(path == null || rand.getRand(0.0, 1.0) <= recalcChanceTimeSpeed){
			//System.out.println(cyclesSinceLastRecalc+" "+recalcChanceTime+" "+recalcChanceTimeSpeed+"   speed:"+currentSpeed+"/"+freeSpeed);
			lastRecalc = cycles;
			calcWholePath(sGB);
		}
		
		//get closest points of all obstacles
		obsPoints = new RadialFace[simpleObs.size()];
		obsDistances.clear();
		for(int i = 0; i < simpleObs.size();i++){
			obsDistances.add(simpleObs.get(i).getClosestPoint(pos));
			obsPoints[i] = new RadialFace(pos,simpleObs.get(i).getClosestPoint(pos));
		}
		Arrays.sort(obsPoints);
		
		//create patharea
		pathArea.reset(pos);
		pathArea.addAll(obsPoints);
		
		//find intersection between patharea border and path
		//Pnt closestPathPoint = path.getLastPoint();
		Pnt borderPoint = null;
		if(path != null){
			outer:
			for(int i = path.getPointsAmount()-2;i >= 0;i--){
				Line2 pathLine = new Line2(path.getPointsOnEdge()[i],path.getPointsOnEdge()[i+1]);
				//System.out.println(i+" "+pathLine);		
				for(Line2 BM : pathArea.getAreaBorderLines()){
					Pnt potBorderPoint = pathLine.intersectionPoint(BM);
					if(potBorderPoint != null){
						if(borderPoint == null){
							borderPoint = potBorderPoint;
						} else {
							if(new Line2(potBorderPoint,pathLine.getEnd()).length() < new Line2(borderPoint,pathLine.getEnd()).length()){
								borderPoint = potBorderPoint;
							}
						}
					}
					//System.out.println("   borderLine:"+BM);					
				}
				if(borderPoint != null){
					if(pathArea.contains(path.getLastPoint())){
						borderPoint = path.getLastPoint();
					}
					break outer;
				}
			}
			if(borderPoint == null){
				borderPoint = path.getLastPoint();
			}
		}
		intermediateGoal = borderPoint;
	}



	private void calcWholePath(SimpleGraphBuilder sGB) {
//		Edge firstEdge = getEdgeClosestPoint(sGB,pos);
//		Edge lastEdge = getEdgeClosestPoint(graph,goal);
		
//		sGB.add(pos);
//		graph.add(goal);
//		sGB.addEdge(pos, firstEdge.getLastPoint(), firstEdge);
//		graph.addEdge(goal, lastEdge.getLastPoint(), lastEdge);
		if(!sGB.getGraph().nodeSet().contains(pos)){
			Pnt oldPos = new Pnt(pos);
			sGB.addPoint(pos);
			if(!sGB.getGraph().nodeSet().contains(pos)){
				System.out.println("fout pos "+oldPos+" "+pos);
			}
			if(!sGB.getGraph().nodeSet().contains(goal)){
				System.out.println("fout goal");
			}
			path = sGB.getGraph().getShortestPath(pos,goal,radius);
			/*if(path == null){
				System.out.println("path null");
			}*/
			sGB.removePoint(pos);
		} else {
			path = sGB.getGraph().getShortestPath(pos,goal,radius);
		}
//		sGB.remove(pos);
//		graph.remove(goal);
	}


	private Edge getEdgeClosestPoint(PointGraph graph, Pnt target) {
		return new Edge(new Pnt[]{target,getClosestPoint(graph,target)});
	}

	private Pnt getClosestPoint(PointGraph graph,Pnt target) {
		double closestDist = Double.MAX_VALUE;
		Pnt closest = null;
		for(Pnt key : graph.getTheNeighbors().keySet()){
			for(Pnt key2 : graph.getTheNeighbors().get(key).keySet()){
				Edge edge = graph.getTheNeighbors().get(key).get(key2);
				for(int i = 0; i < edge.getPointsOnEdge().length;i++){
					double dist = edge.getPointsOnEdge()[i].distanceTo(target);
					if(dist < closestDist){
						closestDist = dist;
						closest = edge.getPointsOnEdge()[i];
					}
				}
			}
		}	
		return closest;
	}

	public void updateCoord(){
		//randtag
		if(rand == null){
			System.out.println("null");
		}
		infAgents.clear();
		//System.out.println(Math.sin(direction)+" "+Math.cos(direction));
		if(path == null){
			FSteer = new Pnt(0.0,0.0);
		} else {
			FSteer = calcFSteer(pos);
		}
		Pnt FStatic = calcFStatic(pos);
		Pnt FDynamic = calcFDynamic(pos,FSteer);
		//System.out.println("FSteer:"+FSteer.magnitude()+" FDynamic:"+FDynamic.magnitude());
		Pnt total = FSteer.add(FStatic).add(FDynamic);
		double randomSpeedScaler = Math.pow(1.1, rand.getGaussian());
		total = new Pnt(total.coord(0) * randomSpeedScaler, total.coord(1) * randomSpeedScaler);
		//restrict to Free Speed
		if(total.magnitude() > freeSpeed/OLSet.getCps()){
			double scaleBackFactor = total.magnitude()/(freeSpeed/OLSet.getCps());
			total = new Pnt(total.coord(0)/scaleBackFactor,total.coord(1)/scaleBackFactor);
		}
		Pnt potPos = pos.add(total);
		//if(potPos > 1.0){
		//	System.out.println("distance "+dist+" = FSteer:"+FSteer.magnitude()+" FStatic:"+FStatic.magnitude()+" FDynamic:"+FDynamic.magnitude());
		//}
		boolean overlapping = false;
		for(Pnt ag : infAgents){
			if(ag.distanceTo(potPos) < 2*radius){
				overlapping = true;
				break;
			}
		}
		for(int i = 0;i < simpleObs.size();i++){
			Pnt pnt = simpleObs.get(i).getClosestPoint(pos);
			if(pnt.distanceTo(pos) < radius){
				overlapping = true;
				break;
			}
		}
		if(!overlapping){
			pos = pos.add(total);
			currentSpeed = total.magnitude()*OLSet.getCps();
		} else {
			currentSpeed = 0.0;
		}
		//pos = pos.add(FStatic);
		//pos = pos.add(FDynamic);
	}

	private Pnt calcFDynamic(Pnt pos, Pnt fSteer) {
		//only process agents that are in goffman ellipse and visible
		ArrayList<AgentRelation> closeAgents = new ArrayList<AgentRelation>();
		for(Agent ag : agents){
			if(ag.getPosition() != this.getPosition()){
				Line2 partialFDynamicLine = new Line2(pos,ag.getPosition());
				if(partialFDynamicLine.length() < goffmanEllipseLength){
					infAgents.add(ag.getPosition());
					Line2 goalLine = new Line2(pos,goal);
					double agentAngle = goalLine.angle()-partialFDynamicLine.angle();
					//only look forward
					//if(Math.abs(agentAngle) < Math.PI/2.0){
						double x = Math.sin(agentAngle)*goffmanEllipseWidth;
						double y = Math.cos(agentAngle)*goffmanEllipseLength;
						Pnt ellipseBorder = new Pnt(x,y);				
						//check if agent is in awareness ellipse and in front
						if(partialFDynamicLine.length() < ellipseBorder.magnitude()){
							double EllipseRatio = goffmanEllipseWidth/goffmanEllipseLength;
							closeAgents.add(new AgentRelation(this,ag,agentAngle,ellipseBorder.magnitude()*EllipseRatio));
						}
					//}
				}
			}
		}	

		Collections.sort(closeAgents);

		ArrayList<AgentRelation> visibleAgents = new ArrayList<AgentRelation>();
		//detect visible agents
		AgentVisibilityLine avl = new AgentVisibilityLine();
		for(AgentRelation ar : closeAgents){
			Agent ag = ar.getRelation();
			//if(avl.addAgent(ar.getMinAngle(), ar.getMaxAngle())){
				visibleAgents.add(ar);
			//}
			//System.out.println(ar.getDistance());
		}
		
		
		
		//sum social force
		//double maxCorridorBoost = 5.0;
		socialPressure = 0.0;
		Pnt FDynamic = new Pnt(0.0,0.0);
		if(visibleAgents.size() >= 4){
			//System.out.println(pos+" "+visibleAgents.size());			
		}
		for(AgentRelation ar : visibleAgents){
			Agent ag = ar.getRelation();
			Line2 partialFDynamicLine = new Line2(pos,ag.getPosition());
			//get direction
			double angle = partialFDynamicLine.angle();
			//div by EllipseBoost makes agent seem closer if the angle is smaller
			double mag = Coxl((partialFDynamicLine.length()-2*radius)/ar.getEllipseBoost())/OLSet.getCps();
			double X = (mag) * Math.sin(angle);
			double Y = (mag) * Math.cos(angle);
			Pnt partialFDynamic = new Pnt(X,Y);

			//avoidance
			double thisSteerAngle = new Line2(new Pnt(0,0),this.getFSteer()).angle();
			double relationSteerAngle = new Line2(new Pnt(0,0),ag.getFSteer()).angle();
			double avoidAngle = (thisSteerAngle+relationSteerAngle)/2;
			double relationAngle = new Line2(this.getPosition(),ag.getPosition()).angle();

			if(Math.abs(relationAngle-avoidAngle) > halfPi){
				avoidAngle += Math.PI;
			}
			
			double avoidScaler = 1.0;
			X = (mag*avoidScaler) * Math.sin(avoidAngle);
			Y = (mag*avoidScaler) * Math.cos(avoidAngle);
			Pnt avoidPartialFDynamic = new Pnt(X,Y);
			
			/*
			System.out.println("----------------------------------");
			System.out.println("thisSteerAngle:"+thisSteerAngle+" relationSteerAngle:"+relationSteerAngle+" avoidAngle:"+avoidAngle+" relationAngle:"+relationAngle);
			System.out.println("thisPos:"+this.getPosition()+" relPos:"+ag.getPosition());
			System.out.println("partialFDynamic:"+partialFDynamic+" avoidPartialFDynamic:"+avoidPartialFDynamic);
			*/
			
			partialFDynamic.add(avoidPartialFDynamic);

			FDynamic = FDynamic.add(partialFDynamic);
			socialPressure += partialFDynamic.magnitude();
			//System.out.println(pos+"/"+ag.getPosition()+" -> "+FDynamic+"/"+partialFDynamic);
		}
		return FDynamic;
	}

	private double Coxl(double distance) {
		double maxAmount = 100;
		double amount = Math.pow(distance/2,-2.0)*0.1;
		if(distance <= 0.0){
			amount = maxAmount;
		}
		if(amount > maxAmount){
			amount = maxAmount;
		}
		//System.out.println("distance:"+distance+" amount:"+amount);
		//if(distance < 0){
		//	System.out.print("distance: "+distance);
		//}
		//System.out.println(" "+amount);
		return amount;
	}

	private Pnt calcFStatic(Pnt pos) {
		//sum static force
		staticPressure = 0.0;
		Pnt FStatic = new Pnt(0.0,0.0);
		for(int i = 0;i < simpleObs.size();i++){
			Pnt closestPoint = simpleObs.get(i).getClosestPoint(pos);
			double db = pos.distanceTo(closestPoint);
			
			if(db < ds+radius){
				Line2 partialFStaticLine = new Line2(pos,closestPoint);
				double angle = partialFStaticLine.angle();
				double X = (cb(db,radius,ds)/OLSet.getCps()) * Math.sin(angle);
				double Y = (cb(db,radius,ds)/OLSet.getCps()) * Math.cos(angle);
				
				Pnt partialFStatic = new Pnt(X,Y);
				FStatic = FStatic.add(partialFStatic);
				staticPressure += partialFStatic.magnitude();
				//System.out.println(pos+"/"+closestPoint+" -> "+FStatic+"/"+partialFStatic);
			}
		}
		//obs.add(pos.add(FStatic));
		return FStatic;
	}

	private double cb(double dbFunc, double radiusFunc, double dsFunc) {
		double k = 2.0;
		return (dsFunc+radiusFunc-dbFunc)/Math.pow((dbFunc-radiusFunc),k);
	}

	private Pnt calcFSteer(Pnt pos2) {
		if(intermediateGoal == null){
			System.out.println();
		}
		Line2 tempLine = new Line2(pos.coord[0],pos.coord[1],intermediateGoal.coord(0),intermediateGoal.coord(1));
		direction = tempLine.angle();
		double X = -(freeSpeed/OLSet.getCps()) * Math.sin(direction);
		double Y = -(freeSpeed/OLSet.getCps()) * Math.cos(direction);
		return new Pnt(X,Y);
	}

	public Shape getDirectionGraphic() {
		return agDirDraw;
	}

	public Shape getPositionGraphic() {
		return agPosDraw;
	}
	
	class EdgePnt {
		public Edge e;
		public int fromIndex;
		public int toIndex;
		public double distance;
		
		public EdgePnt(){
			this.e = null;
			this.fromIndex = 0;
			this.toIndex = 0;
			this.distance = Double.MAX_VALUE;
		}
		
		public Pnt getToPnt(){
			return e.getPointsOnEdge()[toIndex];
		}
		public Pnt getFromPnt(){
			return e.getPointsOnEdge()[fromIndex];
		}
	}

	public Pnt getPosition() {
		return pos;
	}

	public double getRadius() {
		return radius;
	}
	
	public boolean overlaps(Agent other){
		return this.pos.distanceTo(other.pos) <= radius*2.0;
	}
	
	public static void main(String[] blaat){
		RadialFace[] test = new RadialFace[3];
		Pnt origen = new Pnt(10.0,10.0);
		Pnt p1 = new Pnt(0.0,10.0);
		Pnt p2 = new Pnt(2.0,11.0);
		Pnt p3 = new Pnt(20.0,20.0);
		test[0] = new RadialFace(origen,p1);
		test[1] = new RadialFace(origen,p2);
		test[2] = new RadialFace(origen,p3);
		for(int i = 0; i < test.length;i++){
			System.out.println(test[i]);
		}
		System.out.println();
		Arrays.sort(test);
		for(int i = 0; i < test.length;i++){
			System.out.println(test[i]);
		}
	}

	public Pnt getGoal() {
		return goal;
	}

	public void setAgents(ArrayList<Agent> agents) {
		this.agents = agents;
	}
	
	public double getCurrentSpeed(){
		return currentSpeed;
	}

	public void setColor(int colorCode) {
		switch(colorCode){
			case 0 :
				color = Color.green;
				break;
			case 1 :
				color = Color.red;
				break;
			case 2 :
				color = Color.blue;
				break;
			default :
				color = Color.yellow;
				break;
		}
	}
	
	public void setColor(Color color) {
		this.color = color;
	}
	
	public Color getColor(){
		return color;
	}
	
	public UUID getID(){
		return ID;
	}

	public void setPlayback(Pnt pos, Pnt goal, double freeSpeed, double direction, Color color) {
		this.pos = pos;
		this.goal = goal;
		this.freeSpeed = freeSpeed;
		this.direction = direction;
		this.color = color;
	}
}
